#region Using Statements
using System;
using System.Collections.Generic;
using System.Text;
using Microsoft.Xna.Framework;
using JigLibX.Geometry;
using JigLibX.Math;
#endregion

namespace JigLibX.Collision
{
    public class CollDetectCapsuleHeightmap : DetectFunctor
    {

        public CollDetectCapsuleHeightmap()
            : base("CapsuleHeightmap", (int)PrimitiveType.Capsule, (int)PrimitiveType.Heightmap)
        {
        }

        public override void CollDetect(CollDetectInfo infoOrig, float collTolerance, CollisionFunctor collisionFunctor)
        {
            CollDetectInfo info = infoOrig;

            if (info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0).Type == this.Type1)
            {
                CollisionSkin skinSwap = info.Skin0;
                info.Skin0 = info.Skin1;
                info.Skin1 = skinSwap;
                int primSwap = info.IndexPrim0;
                info.IndexPrim0 = info.IndexPrim1;
                info.IndexPrim1 = primSwap;
            }

            Vector3 body0Pos = (info.Skin0.Owner != null) ? info.Skin0.Owner.OldPosition : Vector3.Zero;
            Vector3 body1Pos = (info.Skin1.Owner != null) ? info.Skin1.Owner.OldPosition : Vector3.Zero;

            // todo - proper swept test
            Capsule oldCapsule = (Capsule)info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0);
            Capsule newCapsule = (Capsule)info.Skin0.GetPrimitiveNewWorld(info.IndexPrim0);

            Heightmap oldHeightmap = (Heightmap)info.Skin1.GetPrimitiveOldWorld(info.IndexPrim1);
            Heightmap newHeightmap = (Heightmap)info.Skin1.GetPrimitiveNewWorld(info.IndexPrim1);

            List<CollPointInfo> pts = new List<CollPointInfo>(2);
            Vector3 averageNormal = Vector3.Zero;

            // the start
            {
                float oldDist, newDist;
                Vector3 normal;
                oldHeightmap.GetHeightAndNormal(out oldDist, out normal, oldCapsule.Position);
                newHeightmap.GetHeightAndNormal(out newDist, out normal, newCapsule.Position);

                if (MathHelper.Min(newDist, oldDist) < collTolerance + newCapsule.Radius)
                {
                    float oldDepth = oldCapsule.Radius - oldDist;
                    // calc the world position based on the old position(s)
                    Vector3 worldPos = oldCapsule.Position - oldCapsule.Radius * normal;
                    pts.Add(new CollPointInfo(worldPos - body0Pos, worldPos - body1Pos, oldDepth));
                    averageNormal += normal;
                }
            }
            // the end
            {
                Vector3 oldEnd = oldCapsule.GetEnd();
                Vector3 newEnd = newCapsule.GetEnd();
                float oldDist, newDist;
                Vector3 normal;
                oldHeightmap.GetHeightAndNormal(out oldDist, out normal, oldEnd);
                newHeightmap.GetHeightAndNormal(out newDist, out normal, newEnd);
                if (MathHelper.Min(newDist, oldDist) < collTolerance + newCapsule.Radius)
                {
                    float oldDepth = oldCapsule.Radius - oldDist;
                    // calc the world position based on the old position(s)
                    Vector3 worldPos = oldEnd - oldCapsule.Radius * normal;
                    pts.Add(new CollPointInfo(worldPos - body0Pos, worldPos - body1Pos, oldDepth));
                    averageNormal += normal;
                }
            }

            if (pts.Count > 0)
            {
                if (averageNormal.LengthSquared() > 0.0f)
                    averageNormal.Normalize();

                collisionFunctor.CollisionNotify(info, averageNormal, pts);
            }


        }


    }
}
